/***********************************************************
  MOR.cpp
  Model Order Reduction Class
  SMART -- Symbolic Moment Analysis for Rapid Timing
  07-01-2008
  (c) Guoyong Shi;  SJTU
  09-11-2008
  Zhigang Hao; SJTU

  Code Design:
  The MOR class implements the following functionalities:
  (1) Orthonormalize the moment vectors using the Gram-Schmidt process.
  (2) Generate the reduced order system matrices: \hat{C}, \hat{G}, \hat{B}.
  (3) Find the time constants of the reduced-order system.
 ***********************************************************/

extern "C" {
#include <stdio.h> 
#include <stdlib.h> 
#include <string.h>
#include <assert.h>
#include <math.h>
}


#include <iostream>
#include <iomanip>
#include <stack>

#include "MOR.h"

#include "SmartGlobal.h"
#include "utility.h"

//#define EQUALIZE_MAGNITUDE


using namespace std;
//extern int SampleNum;


MOR::MOR(int size_f, int size_r, RDD *rdd)
{
  int  i,j;

  FullSize = size_f;
  ReducedSize = size_r;

  MDD = rdd;

  // Allocate memory spaces
  C_hat = new double*[ReducedSize];
  for (i = 0; i < ReducedSize; i++) {
    C_hat[i] = new double[ReducedSize];
  }

  G_hat = new double*[ReducedSize];
  for (i = 0; i < ReducedSize; i++) {
    G_hat[i] = new double[ReducedSize];
  }

  for (i = 0; i < ReducedSize; i++) {
    for (j = 0; j < ReducedSize; j++) {
      C_hat[i][j] = 0;
      G_hat[i][j] = 0;
    }
  }

#ifdef ORIG_MATRIX
  if ((MDD->Get_Graph())->Get_Num_Inds() > 0) {
    G = new double*[2*FullSize];
    C = new double[2*FullSize];

    for (i = 0; i < 2*FullSize; i++) {
      C[i] = 0;
      G[i] = new double[2*FullSize];
      for (j = 0; j < 2*FullSize; j++) {
        G[i][j] = 0;
      }
    }
  } else {
    G = new double*[FullSize];
    C = new double[FullSize];

    for (i = 0; i < FullSize; i++) {
      C[i] = 0;
      G[i] = new double[FullSize];
      for (j = 0; j < FullSize; j++) {
        G[i][j] = 0;
      }
    }
  }
#endif

  VT = new double*[ReducedSize];
  for (i = 0; i < ReducedSize; i++) {
    if ((MDD->Get_Graph())->Get_Num_Inds() > 0) {
      VT[i] = new double[2*FullSize];
    } else {
      VT[i] = new double[FullSize];
    }
  }

  // Because moment[0] is a special vector e_1, it simplifies
  // the projection matrix V a little.
  // Initialized VT[0] = e_1\T and VT[][0] = e_1;
  // i.e., the first row and first column of V are the first basis vectors.
  if ((MDD->Get_Graph())->Get_Num_Inds() > 0) {
    for (i = 0; i < ReducedSize; i++) {
      for (j = 0; j < 2*FullSize; j++) {
        VT[i][j] = 0;
      }
    }
  } else {
    for (i = 0; i < ReducedSize; i++) {
      for (j = 0; j < FullSize; j++) {
        VT[i][j] = 0;
      }
    }
  }

  // Request for moments: moment[0], ... moment[q-1].
  // moment[0] = e_1.
  //  rdd->Request_Moment_Order(ReducedSize - 1);

}


MOR::~MOR()
{
  int i;

  for (i = 0; i < ReducedSize; i++) {
    delete C_hat[i];
  }
  delete C_hat;

  for (i = 0; i < ReducedSize; i++) {
    delete G_hat[i];
  }
  delete G_hat;

  for (i = 0; i < ReducedSize; i++) {
    delete VT[i];
  }
  delete VT;

#ifdef ORIG_MATRIX
  if ((MDD->Get_Graph())->Get_Num_Inds() > 0) {
    for (i = 0; i < 2*FullSize; i++) {
      delete G[i];
    }
    delete G;
    delete C;
  } else {
    for (i = 0; i < FullSize; i++) {
      delete G[i];
    }
    delete G;
    delete C;
  }
#endif
}


// Orthonormalize the moment vectors and produce the projection
// matrix V, which will be used to generate reduced-order matrices:
//   V\T C V;  V\T G V;  V\T B.
// Only need to orthonormalize V(1:q, 1:n).
// The Gram_Schmidt Process: 
//  v(j) := m(j) - \sum_{j=0}^{k-1} <m(j), v(j)> v(j)

void MOR::Gram_Schmidt_RLC()
{
  int  i1, i, j, k;
  double *m;   // for the kth moment
  double alpha;  // for inner product
  double norm;  // for the length of a vector

  // m_c(0)=1, m_l(0)=0;
  for (int i = 0; i < FullSize; i++) {
    VT[0][i] = 1 / sqrt(FullSize);
    VT[0][i+FullSize] = 0;
  }
#if 00
  for (int i = 0; i < 2*FullSize; i++) {
    printf("\n");
    for (int j = 0; j < ReducedSize; j++) {
      printf("  %.3e", VT[j][i]);
    }
  }
#endif
  for (k = 1; k < ReducedSize; k++) {
    // VT[0][i] = 1, first order moment
    m = MDD->Get_Moment(k);
    // get moment for kth order
#if 00
    printf("\n");
    for (int i = 0; i < 2*FullSize; i++) {
      printf("\n");
      for (int j = 0; j < ReducedSize; j++) {
        printf("  %.3e", VT[j][i]);
      }
    }    
#endif

    for (i = 0; i < 2*FullSize; i++) {
#ifdef EQUALIZE_MAGNITUDE  
      VT[k][i] = m[i] / m[0];  // equalize the magnitude
#else
      VT[k][i] = m[i];   // initialize: VT[k] := m;
#endif
    }

#if 0
    printf("\n");
    for (int i = 0; i < 2*FullSize; i++) {
      printf("\n");
      for (int j = 0; j < ReducedSize; j++) {
        printf("  %.3e", VT[j][i]);
      }
    }    
#endif

    // Do orthogonalization.
    // This loop is not entered for k == 1.
    for (j = 0; j < k; j++) {
      // get the inner product: alpha = <m(j), v(j)>
      alpha = 0;
      for (i1 = 0; i1 < 2*FullSize; i1++) {
#ifdef EQUALIZE_MAGNITUDE  
        alpha += VT[k][i1]*VT[j][i1];
#else
        alpha += m[i1]*VT[j][i1];
#endif   
      }
      // get the residue
      for (i = 0; i < 2*FullSize; i++) {
        VT[k][i] -= alpha * VT[j][i];
      }
    }
    // Get the norm of VT[k]
    norm = 0;
    for (i = 0; i < 2*FullSize; i++) {
      norm += VT[k][i] * VT[k][i];
    }
    norm = sqrt(norm);
    if (norm == 0) {
      printf("\nMOR::Gram_Schmidt(): Early termination; norm = 0 at k = %d", k);
      return;
    }

    // Do normalization
    for (i = 0; i < 2*FullSize; i++) {
      VT[k][i] /= norm;  // q*n matrix
    }
  }
}



void MOR::Gram_Schmidt_RC()
{
  int  i1, i, j, k;
  double *m;   // for the kth moment
  double alpha;  // for inner product
  double norm;  // for the length of a vector

  for (i = 0; i < FullSize; i++) {
    VT[0][i] = 1 / sqrt(FullSize);
  }
  for (k = 1; k < ReducedSize; k++) {
    // VT[0][i] = 1, first order moment
    m = MDD->Get_Moment(k);
    // get moment for kth order
    for (i = 0; i < FullSize; i++) {
#ifdef EQUALIZE_MAGNITUDE  
      VT[k][i] = m[i] / m[0];  // equalize the magnitude
#else
      VT[k][i] = m[i];   // initialize: VT[k] := m;
#endif   
    } 
    // Do orthogonalization.
    // This loop is not entered for k == 1.
    for (j = 0; j < k; j++) {
      // get the inner product: alpha = <m(j), v(j)>
      alpha = 0;
      for (i1 = 0; i1 < FullSize; i1++) {
#ifdef EQUALIZE_MAGNITUDE  
        alpha += VT[k][i1]*VT[j][i1];
#else
        alpha += m[i1]*VT[j][i1];
#endif   
      }
      // get the residue
      for (i = 0; i < FullSize; i++) {
        VT[k][i] -= alpha * VT[j][i];
      }
    }
    // Get the norm of VT[k]
    norm = 0;
    for (i = 0; i < FullSize; i++) {
      norm += VT[k][i] * VT[k][i];
    }
    norm = sqrt(norm);
    if (norm == 0) {
      printf("\nMOR::Gram_Schmidt(): Early termination; norm = 0 at k = %d", k);
      return;
    }

    // Do normalization
    for (i = 0; i < FullSize; i++) {
      VT[k][i] /= norm;  // q*n matrix
    }
  }
}



/*
   void MOR::Gram_Schmidt()
   {
   int  i1, i, j, k;
   double *m;   // for the kth moment
   double alpha;  // for inner product
   double norm;  // for the length of a vector

   for (k = 1; k < ReducedSize; k++) {
// VT[0][i] = 1, first order moment
m = MDD->Get_Moment(k);
// get moment for kth order
for (i = 1; i < FullSize; i++) {
#ifdef EQUALIZE_MAGNITUDE  
VT[k][i] = m[i] / m[1];  // equalize the magnitude
#else
VT[k][i] = m[i];   // initialize: VT[k] := m;
#endif   
} 
// Do orthogonalization.
// This loop is not entered for k == 1.
for (j = 1; j < k; j++) {
// get the inner product: alpha = <m(j), v(j)>
alpha = 0;
for (i1 = 1; i1 < FullSize; i1++) {
#ifdef EQUALIZE_MAGNITUDE  
alpha += VT[k][i1]*VT[j][i1];
#else
alpha += m[i1]*VT[j][i1];
#endif   
}
// get the residue
for (i = 1; i < FullSize; i++) {
VT[k][i] -= alpha * VT[j][i];
}
}
// Get the norm of VT[k]
norm = 0;
for (i = 1; i < FullSize; i++) {
norm += VT[k][i] * VT[k][i];
}
norm = sqrt(norm);
if (norm == 0) {
printf("\nMOR::Gram_Schmidt(): Early termination; norm = 0 at k = %d", k);
return;
}

// Do normalization
for (i = 1; i < FullSize; i++) {
VT[k][i] /= norm;  // q*n matrix
}
}
}
*/

// Create reduced system matrices: C_hat = V\T C V;  G_hat = V\T G V;
// To save the computation cost, no explicit matrices C and G are created.
// C_hat and G_hat are computed by one Breath-First traversal of the C-Tree.
// Algorithm:
//   C_hat = \sum_{i=1}^n C_i (V[][i])(V[][i])\T
//   G_hat = G_i (V[][1])(V[][1])\T + 
//              \sum_{i=1}^n G_i (V[][p(i)]-V[][i])(V[][p(i)]-V[][i])\T
// p(i) is the parent node of node "i".

void MOR::Reduce_C_G_RC(C_NODE *ctree)
{
  Queue_Node *head = NULL, *tail = NULL, *qnode; // for queueing
  int   i, j;  // matrix entry indexes
  int   p;  // the parent node number
  int   k;  // the current node number
  double  g;  // temp conductance

#ifdef TRACE
  printf("\nMOR::Reduce_C_G_RC() : C-Tree root [%s]", ctree->elemC->name);
#endif

  if (!ctree)  return;  // do nothing

  // Enqueue the C-Tree root node
  qnode = new Queue_Node;
  qnode->nodeC = ctree;
  qnode->next = NULL;
  head = tail = qnode;
  // Start from ctree, C1*w1*w1\T if C1 is ctree root node
  // Initialized C_hat & G_hat
  k = qnode->nodeC->number; // get the node number
  for (i = 0; i < ReducedSize; i++) {
    for (j = 0; j < ReducedSize; j++) {
      C_hat[i][j] = qnode->nodeC->elemC->value[1]*VT[i][k]*VT[j][k];
      g = 1 / qnode->nodeC->elemR->value[1];
      G_hat[i][j] = g*VT[i][k]*VT[j][k];
    }
  }

#ifdef ORIG_MATRIX
  // Set the C matrix diagonal and G matrix
  C[k] += qnode->nodeC->elemC->value[1];

#if 00
  cout << "k" <<k << "C[k]" << C[k]<<endl;
#endif

  G[k][k] += 1 / qnode->nodeC->elemR->value[1];
#endif

  // Set the parent node 
  p = k;   
  // Loop until no Queue_Node left in the queue.
  while (1) { //breadth first searching algorithm
    for (int n = 0;  n < head->nodeC->fanout; n++)
    {
      //#ifdef TRACE
#if 0
      printf("\n... working on C-Tree node [%s] --> p = %d",  
          head->nodeC->child[n]->elemC->name, p);
#endif
      // ** Enqueue --- 
      // Put the siblings of the current C-Tree node in queue.
      qnode = new Queue_Node;
      qnode->nodeC = head->nodeC->child[n];
      qnode->next = NULL;
      tail->next = qnode;
      tail = qnode;
      // Accumulate C_hat & G_hat
      k = qnode->nodeC->number; // get the node number
      for (i = 0; i < ReducedSize; i++) {
        for (j = 0; j < ReducedSize; j++) {
          C_hat[i][j] += qnode->nodeC->elemC->value[1]*VT[i][k]*VT[j][k];
          g = 1 / qnode->nodeC->elemR->value[1];
          G_hat[i][j] += g*(VT[i][k]-VT[i][p])*(VT[j][k]-VT[j][p]);
        }
      }

#ifdef ORIG_MATRIX

      // Set the C matrix diagonal and G matrix
      C[k] += qnode->nodeC->elemC->value[1];
      //cout << "k" <<k <<"p"<<p<< "C[k]" << C[k]<<endl;

      G[k][k] += 1 / qnode->nodeC->elemR->value[1];
      G[k][p] -= 1 / qnode->nodeC->elemR->value[1];
      G[p][k] -= 1 / qnode->nodeC->elemR->value[1];
      G[p][p] += 1 / qnode->nodeC->elemR->value[1];
#endif

    }



    // Delete the node at queue head and advance the head.
    qnode = head;
    head = head->next; // do the same breadth first search of the firstchild
    // of head
    delete qnode;
    if (!head) break; 
    // No more node in the queue; terminate the loop.

    // ** Dequeue --- 
    // Reset the parent node
    p = head->nodeC->number;
  }
}

// For RLC cases
void MOR::Reduce_C_G_RLC(C_NODE *ctree)
{
  Queue_Node *head = NULL, *tail = NULL, *qnode; // for queueing
  int   i, j;  // matrix entry indexes
  int   p;  // the parent node number
  int   k;  // the current node number
  double  g;  // temp conductance

#ifdef TRACE
  printf("\nMOR::Reduce_C_G_RLC() : C-Tree root [%s]", ctree->elemC->name);
#endif
  if (!ctree)  return;  // do nothing
  // Enqueue the C-Tree root node
  qnode = new Queue_Node;
  qnode->nodeC = ctree;
  qnode->next = NULL;
  head = tail = qnode;
  // Start from ctree, C1*w1*w1\T if C1 is ctree root node
  // Initialized C_hat & G_hat
  // For RLC, C = diag(Ci, Li), G = [0 A; -A\T R],
  k = qnode->nodeC->number; // get the node number
  //cout << "k:" << k << endl;
  for (i = 0; i < ReducedSize; i++) {
    for (j = 0; j < ReducedSize; j++) {
      C_hat[i][j] += qnode->nodeC->elemC->value[1]*VT[i][k]*VT[j][k];
      C_hat[i][j] += qnode->nodeC->elemL->value[1]*VT[i][k+FullSize]*VT[j][k+FullSize];
      //      g = 1 / qnode->nodeC->elemR->value[1];
      G_hat[i][j] += VT[i][k]*VT[j][k+FullSize];
      G_hat[i][j] -= VT[j][k]*VT[i][k+FullSize];
      G_hat[i][j] += qnode->nodeC->elemR->value[1]*VT[i][k+FullSize]*VT[j][k+FullSize];
    }
  }
#ifdef ORIG_MATRIX
  C[k] += qnode->nodeC->elemC->value[1];
  C[k+FullSize] += qnode->nodeC->elemL->value[1];

  //cout << "k" <<k <<"p"<<p<< "C[k]" << C[k]<<endl;

  G[k+FullSize][k+FullSize] += qnode->nodeC->elemR->value[1];
  G[k][k+FullSize] += 1;
  G[k+FullSize][k] += -1;
#endif

  // Set the parent node 
  p = k;
  // Loop until no Queue_Node left in the queue.
  while (1) { //breadth first searching algorithm
    for (int n = 0;  n < head->nodeC->fanout; n++)
    {
      //#ifdef TRACE
#if 0
      printf("\n... working on C-Tree node [%s] --> p = %d",  
          head->nodeC->child[n]->elemC->name, p);
#endif
      // ** Enqueue --- 
      // Put the siblings of the current C-Tree node in queue.
      qnode = new Queue_Node;
      qnode->nodeC = head->nodeC->child[n];
      qnode->next = NULL;
      tail->next = qnode;
      tail = qnode;
      // Accumulate C_hat & G_hat
      k = qnode->nodeC->number; // get the node number
      for (i = 0; i < ReducedSize; i++) {
        for (j = 0; j < ReducedSize; j++) {
          C_hat[i][j] += qnode->nodeC->elemC->value[1]*VT[i][k]*VT[j][k];
          C_hat[i][j] += qnode->nodeC->elemL->value[1]*VT[i][k+FullSize]*VT[j][k+FullSize];
          G_hat[i][j] += qnode->nodeC->elemR->value[1]*VT[i][k+FullSize]*VT[j][k+FullSize];
          //cout << "G_hat["<<i<<"]["<<j<<"] orig " <<G_hat[i][j]<< endl;
          //cout << "i j k p" << i << j << k << p << endl;
          //cout << " += "<<VT[i][k]<<" - "<<VT[i][p]<<
          // "*"<<VT[j][k+FullSize]<< endl;
          G_hat[i][j] += (VT[i][k]-VT[i][p])*VT[j][k+FullSize];
          //        cout <<"after"<<G_hat[i][j]<< endl;
          G_hat[i][j] -= (VT[j][k]-VT[j][p])*VT[i][k+FullSize];
        }
      }
#ifdef ORIG_MATRIX

      C[k] += qnode->nodeC->elemC->value[1];
      C[k+FullSize] += qnode->nodeC->elemL->value[1];

      //cout << "k" <<k <<"p"<<p<< "C[k]" << C[k]<<endl;

      G[k+FullSize][k+FullSize] += qnode->nodeC->elemR->value[1];
      G[p][k+FullSize] += -1;
      G[k][k+FullSize] += 1;

      G[k+FullSize][k] += -1;
      G[k+FullSize][p] += 1;

#endif
    }
    // Delete the node at queue head and advance the head.
    qnode = head;
    head = head->next; // do the same breadth first search of the firstchild
    // of head
    delete qnode;
    if (!head) break; 
    // No more node in the queue; terminate the loop.
    // ** Dequeue --- 
    // Reset the parent node
    p = head->nodeC->number;
  }

}




// Print the projection matrix V.
void MOR::Print_V(FILE *file)
{
  //fprintf(file, "\nThe projection matrix V is: ");
  if ((MDD->Get_Graph())->Get_Num_Inds() > 0) {
    for (int i = 0; i < 2*FullSize; i++) {
      fprintf(file, "\n");
      for (int j = 0; j < ReducedSize; j++) {
        fprintf(file, "  %.10e", VT[j][i]);
      }
    }

  } else {
    for (int i = 0; i < FullSize; i++) {
      fprintf(file, "\n");
      for (int j = 0; j < ReducedSize; j++) {
        fprintf(file, "  %.10e", VT[j][i]);
      }
    }
  }
  fprintf(file, "\n");
}

/*
// Print the projection matrix V.
void MOR::Print_Reduced_V(FILE *file)
{
//fprintf(file, "\nThe projection matrix V is: ");
if ((MDD->Get_Graph())->Get_Num_Inds() > 0) {
for (int i = 0; i < 2*FullSize; i++) {
fprintf(file, "\n");
for (int j = 0; j < ReducedSize; j++) {
fprintf(file, "  %.10e", VT[j][i]);
}
}

} else {
for (int i = 0; i < FullSize; i++) {
fprintf(file, "\n");
for (int j = 0; j < ReducedSize; j++) {
fprintf(file, "  %.10e", VT[j][i]);
}
}
}
fprintf(file, "\n");
}
*/

// Print the reduced matrices C_hat & G_hat.
void MOR::Print_Reduced_C_G(FILE *file)
{
  int  i, j;
  double tmp_f_hat, tmp_r_root;

  //fprintf(file, "\nThe reduced matrix C_hat is: ");
  for (i = 0; i < ReducedSize; i++) {
    fprintf(file, "\n");
    for (j = 0; j < ReducedSize; j++) {
      fprintf(file, "  %.10e", C_hat[i][j]);
    }
  }
  fprintf(file, "\n");

  //fprintf(file, "\nThe reduced matrix G_hat is: ");
  for (i = 0; i < ReducedSize; i++) {
    fprintf(file, "\n");
    for (j = 0; j < ReducedSize; j++) {
      fprintf(file, "  %.10e", G_hat[i][j]);
    }
  }
  fprintf(file, "\n");
  //For RC cases Fr = G1*V(1,:) 
  if ((MDD->Get_Graph())->Get_Num_Inds() == 0) {
    fprintf(file, "\n");
    tmp_r_root =MDD->Get_Graph()->Get_SrcNode()->elem_p->elem->value[1];
    for (j = 0; j < ReducedSize; j++) {
      tmp_f_hat = VT[j][0]/tmp_r_root;
      fprintf(file, "  %.10e", tmp_f_hat);
    }
    fprintf(file, "\n");
  }
}

#ifdef ORIG_MATRIX
// Print the original matrices C & G.
void MOR::Print_Orig_C_G(FILE *file)
{
  int  i, j;
  if ((MDD->Get_Graph())->Get_Num_Inds() > 0) {
    //fprintf(file, "\nThe original matrix C is: ");
    fprintf(file, "\n");
    for (i = 0; i < 2*FullSize; i++) {
      fprintf(file, "  %.3e", C[i]);
    }
    fprintf(file, "\n");

    //fprintf(file, "\nThe original matrix G is: ");
    for (i = 0; i < 2*FullSize; i++) {
      fprintf(file, "\n");
      for (j = 0; j < 2*FullSize; j++) {
        fprintf(file, "  %.3e", G[i][j]);
      }
    }
    fprintf(file, "\n");
  } else {
    //fprintf(file, "\nThe original matrix C is: ");
    fprintf(file, "\n");
    for (i = 0; i < FullSize; i++) {
      fprintf(file, "  %.3e", C[i]);
    }
    fprintf(file, "\n");

    //fprintf(file, "\nThe original matrix G is: ");
    for (i = 0; i < FullSize; i++) {
      fprintf(file, "\n");
      for (j = 0; j < FullSize; j++) {
        fprintf(file, "  %.3e", G[i][j]);
      }
    }
    fprintf(file, "\n");

  }
}
#endif

